/**
 * This class contains the major sever-client commands interpretation and execution code
 */

package com.mcp_31091.roomba.Views;

import static com.mcp_31091.roomba.RoombaActivity.btThread;
import static com.mcp_31091.roomba.connection.ConnectedThread.MSG_MOVE_SAMPLE;
import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.FileNotFoundException;
import java.io.IOException;
import java.io.InputStream;
import java.io.InputStreamReader;
import java.io.OutputStreamWriter;
import java.io.PrintWriter;
import java.net.ServerSocket;
import java.net.Socket;
import java.net.UnknownHostException;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import android.app.Activity;
import android.content.Context;
import android.net.wifi.ScanResult;
import android.net.wifi.WifiManager;
import android.os.Bundle;
import android.os.Message;
import android.util.Log;
import android.view.View;
import android.widget.Button;
import android.widget.EditText;
import android.widget.TextView;
import android.widget.Toast;
import com.mcp_31091.R;
import com.mcp_31091.roomba.RoutePlan.RoutePlanner;
import com.mcp_31091.roomba.WifiSignalMap.MapGenerator;
import com.mcp_31091.roomba.WifiSignalMap.MapPoint;
import com.mcp_31091.roomba.helper.Helper;
import com.mcp_31091.roomba.localizer.Localizer;

public class CopyOfRoombaCommandsView extends Activity {
	private final String SERVER_IP = "172.19.11.193";
	public static final int REQUEST_ENABLE_BT = 0;
	public static final String TAG = "DebugInfo";
	protected static final char CMD_MOVE_FORWARD = 0x01;
	protected static final char CMD_MOVE_BACKWARD = 0x02;
	protected static final char CMD_SPIN_LEFT = 0x03;
	protected static final char CMD_SPIN_RIGHT = 0x04;
	protected static final char CMD_SEND_WIFI = 0x05;
	private Button driveForwardButton;
	private Button driveBackwardButton;
	private int yPos = 0, xPos = 0; // because the x and y coordinates are in
									// milli-meters, just use integer should be fine
	private float turningRadius = 0;
	private double orientation;
	private WifiManager wifi;
	private Button spinLeftButton;
	private Button spinRightButton;
	private EditText driveForwardText;
	private EditText driveBackwardText;
	private Button setSpeedButton;
	private EditText setSpeedText;
	private Button resetButton;
	private Button showInfoButton;
	private TextView showInfoText;
	private Button initButton;
	private EditText initXText;
	private EditText initYText;
	private EditText initOText;
	private int counter = 0;

	@Override
	protected void onCreate(Bundle savedInstanceState) {
		super.onCreate(savedInstanceState);
		setContentView(R.layout.test_drive_panel);
		setupViews();
		setupServer();
		wifi = (WifiManager) getSystemService(Context.WIFI_SERVICE);
	}

	private void setSpeed(int speed) {
		btThread.speed = speed;
	}

	public void goStraight(int distance) {
		int pausetime = Math.abs(distance * 1000 / btThread.speed); // mm/(mm/sec)
																	// = sec
		int remainder = pausetime % 1000;

		if (distance > 0) // forward
		{
			btThread.goStraightAt(btThread.speed);
			for (int i = 0; i < pausetime / 1000; i++) {
				btThread.pause(1000);
				updatePos(btThread.speed);
			}
		} else // backward
		{
			btThread.goStraightAt(-btThread.speed);
			for (int i = 0; i < pausetime / 1000; i++) {
				btThread.pause(1000);
				updatePos(-1 * btThread.speed);
			}
		}

		if (remainder != 0) {
			btThread.pause(remainder);
//			System.out.println("remainder*distance/time is: " + remainder
//					* distance / pausetime);
			updatePos(remainder * distance / pausetime);
		}
		btThread.stopDrive();
	}

	int preX = 0;
	int preY = 0;
	private Button sendWifiButton;
	private Button spinLeftAtButton;
	private EditText spinLeftAtText;
	private Button senseButton;
	private EditText senseText;
	private Button driveButton;
	private int currentX = 0;
	private int currentY = 0;

	private void updatePos(int distance) {
		System.out.println("distance is " + distance);
		System.out.println("xPos begin" + xPos);
		xPos += (int) (distance * Math.cos(orientation * 2 / 360 * Math.PI));
		yPos += (int) (distance * Math.sin(orientation * 2 / 360 * Math.PI));
		System.out
				.println("(int) (distance * Math.cos(orientation*2/360*Math.PI)) is"
						+ (int) (distance * Math.cos(orientation * 2 / 360
								* Math.PI)));
		System.out.println("xPos " + xPos);
	}

	public void move(double turn, int forward) {
		if (forward == 0)
			return;
		if (turn != 0)
			spinCalculation(turn);
		// if(forward > 0)
		// btThread.goForward(forward);
		// if(forward < 0)
		// {
		// btThread.goBackward(-1*forward);
		// // Toast.makeText(this, "going backwards", Toast.LENGTH_LONG).show();
		// }
		// btThread.stopDrive();

		orientation += turn;// + new Random().nextGaussian()*turn_noise;
		orientation %= 360;
		int distance = forward; // + new Random().nextGaussian()*forward_noise;
		xPos += (int) (distance * Math.cos(orientation * 2 / 360 * Math.PI));
		// Toast.makeText(this, "Math.cos(orientation*2/360*Math.PI) is " +
		// Math.cos(orientation*2/360*Math.PI), Toast.LENGTH_LONG).show();
		System.out.println("xpos Math.cos(orientation*2/360*Math.PI) is "
				+ (int) Math.cos(orientation * 2 / 360 * Math.PI));
		yPos += (int) (distance * Math.sin(orientation * 2 / 360 * Math.PI));
		System.out.println("ypos Math.cos(orientation*2/360*Math.PI) is "
				+ (int) Math.sin(orientation * 2 / 360 * Math.PI));
	}

	void setupViews() {
		setSpeed(150); // a little fast
		// spinButton = (Button)findViewById(R.id.spin_button);
		sendWifiButton = (Button) findViewById(R.id.send_wifi_button);
		driveButton = (Button) findViewById(R.id.drive_button);

		senseButton = (Button) findViewById(R.id.sense_button);
		senseText = (EditText) findViewById(R.id.sense_text);

		setSpeedButton = (Button) findViewById(R.id.set_speed_button);
		setSpeedText = (EditText) findViewById(R.id.set_speed_text);

		driveForwardButton = (Button) findViewById(R.id.drive_forward_button);
		driveForwardText = (EditText) findViewById(R.id.drive_forward_text);
		driveForwardText.setText("500");

		driveBackwardButton = (Button) findViewById(R.id.drive_backward_button);
		driveBackwardText = (EditText) findViewById(R.id.drive_backward_text);
		driveBackwardText.setText("500");

		spinLeftButton = (Button) findViewById(R.id.spin_left_button);
		spinRightButton = (Button) findViewById(R.id.spin_right_button);

		showInfoButton = (Button) findViewById(R.id.show_info_button);
		showInfoText = (TextView) findViewById(R.id.info_text);

		spinLeftAtButton = (Button) findViewById(R.id.spin_left_at_button);
		spinLeftAtText = (EditText) findViewById(R.id.spin_left_at_text);

		initButton = (Button) findViewById(R.id.init_button);
		initXText = (EditText) findViewById(R.id.init_x_text);
		initYText = (EditText) findViewById(R.id.init_y_text);
		initOText = (EditText) findViewById(R.id.init_o_text);

		resetButton = (Button) findViewById(R.id.reset_button);

		driveButton.setOnClickListener(new View.OnClickListener() {

			public void onClick(View v) {
				Toast.makeText(CopyOfRoombaCommandsView.this, "drive", Toast.LENGTH_LONG)
						.show();

				btThread.newDrive(21500);
				spinRight();
				btThread.newDrive(3500);
				spinLeft();
				btThread.newDrive(17500);
				spinLeft();
				btThread.newDrive(4700);
				spinRight();
				btThread.newDrive(30000);
				spinLeft();
				btThread.newDrive(8000);
				spinRight();
				btThread.newDrive(5000);
				spinRight();
				btThread.newDrive(6400);
				spinRight();
				btThread.newDrive(31000);
				spinLeft();
				btThread.newDrive(11400);
				spinRight();
				btThread.newDrive(1000);
				spinRight();
				btThread.newDrive(6000);
				spinLeft();
				btThread.newDrive(21500);
				spinLeft();
				btThread.newDrive(6000);
				spinRight();
				btThread.newDrive(1500);
				spinRight();
				btThread.newDrive(10000);
				spinLeft();
				btThread.newDrive(18000);
			}
		});

		setSpeedButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				setSpeed(Integer.parseInt(setSpeedText.getText().toString()));
			}
		});

		senseButton.setOnClickListener(new View.OnClickListener() {

			public void onClick(View v) {
				btThread.start();
				// int packetId =
				// Integer.parseInt(senseText.getText().toString());
				// byte[] cmd = new byte[46];
				//
				// cmd[0] = (byte)148;
				// cmd[1] = (byte)43;
				//
				// for(int i = 0; i<43; i++)
				// {
				// cmd[i+2] = (byte)i;
				// }

				byte[] cmd = { (byte) 148, (byte) 5, (byte) 19, (byte) 20,
						(byte) 24, (byte) 25, (byte) 26 };

				btThread.send(cmd);
				try {
					Thread.sleep(1000);
				} catch (InterruptedException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}

				byte[] a = btThread.getBytes();

				for (int i = 0; i < 20; i++) {
					System.out.println((int) a[i]);
				}
			}
		});

		// driveForwardButton.setOnClickListener(new View.OnClickListener(){
		// public void onClick(View v) {
		// // move(0, Integer.parseInt(driveForwardText.getText().toString()));
		// //111
		//
		// new Thread(new Runnable() {
		// public void run() {
		// int forwardDistance =
		// Integer.parseInt(driveForwardText.getText().toString());
		// int forwardDistanceRemainder = forwardDistance%500;
		// for(int i = 0; i < forwardDistance/500; ++i)
		// {
		// move(0, 500);
		// sendCords();
		// }
		// if(forwardDistanceRemainder != 0)
		// {
		// System.out.println("remainder is: " + forwardDistanceRemainder);
		// move(0, forwardDistanceRemainder);
		// sendCords();
		// }
		// }
		// }).start();
		// }
		//
		// });
		// class SendCordThread extends Thread
		// {
		// private Handler sendCordHandler ;
		// private boolean stop = false;
		//
		//
		// public void run() {
		// Looper.prepare();
		// sendCordHandler = new Handler(){
		// public void handleMessage(Message msg)
		// {
		// if(!stop)
		// sendCords();
		// switch(msg.what)
		// {
		// case 0:
		// while(true)
		// System.out.println("hello");
		// break;
		// case 1:
		// System.out.println("finished");
		// stop = true;
		// break;
		// }
		// }
		//
		// };
		// System.out.println("started");
		// Looper.loop();
		// }
		//
		// public Handler getHandler()
		// {
		// return sendCordHandler;
		// }
		// };

		class SendCordThread extends Thread {
			public void run() {
				while (!isInterrupted()) {
					for (int i = 0; i < 10000; i++) // delay some time
					{
						if (i == 3000) {
							System.out.println(i);
						}
					}

					if ((preX != xPos) || (preY != yPos)) // if a different
															// point
					{
						sendCords();
						preX = xPos;
						preY = yPos;
					}
				}
				sendCords();
			}
		}
		;

		driveForwardButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				// move(0,
				// Integer.parseInt(driveForwardText.getText().toString()));
				final SendCordThread sendCordThread = new SendCordThread();
				sendCordThread.start();
				int forwardDistance = Integer.parseInt(driveForwardText
						.getText().toString());
				btThread.newDrive(forwardDistance);
				move(0, forwardDistance);
				sendCordThread.interrupt();
			}
		});

		driveBackwardButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				final SendCordThread backwardThread = new SendCordThread();
				backwardThread.start();
				int backwardDistance = Integer.parseInt(driveBackwardText
						.getText().toString());
				btThread.newDrive(-1 * backwardDistance);
				move(0, -1 * backwardDistance);
				backwardThread.interrupt();
			}
		});

		spinLeftButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				spinLeft();
			}
		});
		spinRightButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				spinRight();
			}
		});

		spinLeftAtButton.setOnClickListener(new View.OnClickListener() {

			public void onClick(View v) {
				System.out.println("spined");
				System.out.println(Integer.parseInt(spinLeftAtText.getText()
						.toString()));

				btThread.newTurn(Integer.parseInt(spinLeftAtText.getText()
						.toString()));
			}
		});

		showInfoButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				showInfoText.setText("counter: " + counter + "x:" + xPos
						+ " y:" + yPos + " orientatioin:" + orientation);
			}
		});

		initButton.setOnClickListener(new View.OnClickListener() {

			public void onClick(View v) {
				xPos = Integer.parseInt(initXText.getText().toString());
				yPos = Integer.parseInt(initYText.getText().toString());
				orientation = Integer.parseInt(initOText.getText().toString());

			}
		});

		resetButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				reset();
			}
		});

		sendWifiButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				new Thread(new Runnable() {
					public void run() {
						sendWifiSignal();
					}
				}).start();
			}
		});
	}

	void reset() {
		orientation = 0;
		xPos = 0;
		yPos = 0;
		counter = 0;
	}

	void spinLeft() {
		// btThread.spinLeft(90);
		btThread.newTurn(85);
		orientation += 90;// + new Random().nextGaussian()*turn_noise;
		orientation %= 360;
	}

	void spinRight() {
		// btThread.spinRight(90);
		btThread.newTurn(-85);
		orientation += 360;
		orientation -= 90;
		orientation %= 360;
	}

	private void sendWifiSignal() {
		String signal = "couter," + counter + ",x," + xPos + ",y," + yPos
				+ ", ";
		wifi.startScan();
		List<ScanResult> scanResults = wifi.getScanResults();
		for (ScanResult sr : scanResults) {
			Log.i("SignalTag", sr.toString());
			signal += sr.BSSID + "," + sr.level;
		}
		sendPacket(signal); // send one point of signals at once
		counter++;
	}

	private void sendWifiSignal(PrintWriter out3) {
		String signal = "";
		wifi.startScan();
		List<ScanResult> scanResults = wifi.getScanResults();
		signal += "x:," + xPos + ",y:," + yPos + ",";
		for (ScanResult sr : scanResults) {
			Log.i("SignalTag", sr.toString());
			signal += sr.BSSID + "," + sr.level + ",";
		}
		out3.println(signal); // send one point of signals at once
	}

	private void sendCords() {
		sendPacket("x:," + xPos + ",y:," + yPos);
	}

	public void spinCalculation(double degree) {
		float pausetime = 100 / 1000 + (float) Math
				.abs(((float) (230 * Math.PI / 360.0)) * 103.8 / 100);
		int realpause = (int) (pausetime * 1000 * degree / 90);
		btThread.drive(100, 1);
		btThread.pause(realpause);
		btThread.stopDrive();
	}

	public void spin(View view) {
		orientation = 0;
		xPos = 0;
		yPos = 0;
		for (int i = 0; i < 12; i++) {
			move(0, 500);
			new Thread(new Runnable() {
				public void run() {
					sendWifiSignal();
				}
			}).start();
		}
	}

	private void sendPacket(String packet) {
		try {
			Socket socket = new Socket(SERVER_IP, 12345);
			PrintWriter out = new PrintWriter(new BufferedWriter(
					new OutputStreamWriter(socket.getOutputStream())), true);
			out.println(packet);

			BufferedReader in = new BufferedReader(new InputStreamReader(
					socket.getInputStream()));

		} catch (UnknownHostException e) {
			e.printStackTrace();
		} catch (IOException e) {
			e.printStackTrace();
		}
	}
	
	/*Setup a server to communicate with the processing client program*/
	private void setupServer() {
		try {
			new Thread(new Runnable() {

				BufferedReader bufferedReader;
				ServerSocket serverSocket = new ServerSocket(12345);
				Socket clientSocket = null;
				PrintWriter out;

				public void run() {
					while (true) {
						try {
							clientSocket = serverSocket.accept();
							// Create a reader
							try {
								bufferedReader = new BufferedReader(
										new InputStreamReader(clientSocket
												.getInputStream()));
								out = new PrintWriter(new BufferedWriter(
										new OutputStreamWriter(clientSocket
												.getOutputStream())), true);

								out.println("client ip is "
										+ clientSocket.getLocalAddress());
								// int cmd = bufferedReader. read(); //read one
								// byte
								String s;
								while ((s = bufferedReader.readLine()) != null) {

									String[] temp = s.split(",");
									Log.e("LINE",s);
									if (temp.length > 0) {
										if (temp[0].equals("forward")) {  //Foward commmand

											btThread.newDrive(Integer
													.parseInt(temp[1]));
											move(0, Integer.parseInt(temp[1]));

										} else if (temp[0].equals("backward")) { //Backward command

											btThread.newDrive(-1
													* Integer.parseInt(temp[1]));
											move(0,
													-1
															* Integer
																	.parseInt(temp[1]));
										} else if (temp[0].equals("spinLeft")) {  //Spinleft command

											spinLeft();
										} else if (temp[0].equals("spinRight")) {  //SpinRight command

											spinRight();
										} else if (temp[0].equals("setSpeed")) {   //SetSpeed command
											out.println("SPEED set to:"
													+ temp[1]);
											setSpeed(Integer.parseInt(temp[1]));
										} else if (temp[0].equals("sendWifi")) {   //SendWifi command
											// out.println("sendwifi...");
											counter = 0;
											sendWifiSignal(out);
											// out.println("Wifi Scan Complete!!!!");
										} else if (temp[0].equals("clear")) {   //Reset xpos, ypos, orientation command
											xPos = 0;
											yPos = 0;
											orientation = 0;
										} else if (temp[0].equals("set")   // Set xpos, ypos, orientation command
												&& temp.length == 4) {
											xPos = Integer.parseInt(temp[1]);
											yPos = Integer.parseInt(temp[2]);
											orientation = Integer
													.parseInt(temp[3]);
										}else if (temp[0].equals("parseMap"))  //Parse map file command
										{
											out.println("parse map back");
											MapGenerator.getInstance().generateMap();
											//parseMapFileTest();
											
										}else if (temp[0].equals("generateParticles"))  //Generate particles command
										{
											Localizer.getInstance().generateParticles();
										}else if(temp[0].equals("localize"))	//Localize command
										{
											out.println("LOCALIZE " + temp[1]);
											Localizer.getInstance().localize(Integer.parseInt(temp[1]));
										}else if(temp[0].equals("setMap")){	   //Navigation command
											int intx=0,inty=0,destx=0,desty=0;
											if (Integer.parseInt(temp[1])<0)
												intx=Integer.parseInt(temp[1])*-1/1000-1;
											else if (Integer.parseInt(temp[1])==0)
											{
												intx = 0;
											}else{
												intx=Integer.parseInt(temp[1])/1000-1;
											}
											
											if (Integer.parseInt(temp[2])<0){
												inty=Integer.parseInt(temp[2])*-1/1000-1;
											}
											else if (Integer.parseInt(temp[2])==0)
											{
												inty = 0;
											}
											else
											{
												inty=Integer.parseInt(temp[2])/1000-1;
											}
											
											if (Integer.parseInt(temp[3])<0){
												destx=Integer.parseInt(temp[3])*-1/1000-1;
											}
											else if (Integer.parseInt(temp[3])==0)
											{
												destx = 0;
											}
											else
											{
												destx=Integer.parseInt(temp[3])/1000-1;
											}
											
											if (Integer.parseInt(temp[4])<0){
												desty=Integer.parseInt(temp[4])*-1/1000-1;
											}
											else if (Integer.parseInt(temp[4])==0)
											{
												desty = 0;
											}
											else
											{
												desty=Integer.parseInt(temp[4])/1000-1;
											}
											out.println("POINT"+intx+inty+destx+desty);
											
											RoutePlanner.getInstance().setInit(inty,intx);
											RoutePlanner.getInstance().setDest(desty,destx);
											autoDrive();
										}else if(temp[0].equals("sample"))  //Sample command
										{
											System.out.println("GetSampleCMD");
											System.out.println("sample " + temp[1]);
											out.println("SAMPLE " + temp[1]);
											sample(out, Integer.parseInt(temp[1]));
										}
									}
								}
								
							} catch (IOException e) {
								e.printStackTrace();
							}

						} catch (IOException e) {
							System.out.println(e);
						}

						finally {
							if (clientSocket != null) {
								try {
									clientSocket.close();
								} catch (IOException e) {
									e.printStackTrace();
								}
							}
							if (bufferedReader != null) {
								try {
									bufferedReader.close();
								} catch (IOException e) {
									e.printStackTrace();
								}
							}
							if (out != null) {
								out.close();
							}
						}
					}
				}

			}).start();
		} catch (IOException e) {
			e.printStackTrace();
		}
	}
	
	/*Sample to generate the wifi*/
	protected void sample(final PrintWriter out, final int distance) {
		//move foward 1 m
		//stop and send wifi signal
		new Thread(new Runnable() { 
            public void run(){
            	try {
            		while(xPos < 21000)
            		{
            			double startTime = System.currentTimeMillis();
            			Message msg = new Message();
            			msg.what = MSG_MOVE_SAMPLE;
            			msg.arg1 = distance;
            			btThread.bHandler.sendMessage(msg);
            			move(0, distance);
						Thread.sleep(8*distance);
						double endTime  = System.currentTimeMillis();
						System.out.println("Time elapsed is " + (endTime-startTime));
						//System.out.println("OUT of thread!");
						sendWifiSignal(out);
            		}
            		Helper.sendPacket("Sample finished!");
            		
				} catch (InterruptedException e) {
					e.printStackTrace();
				}
            }
    }).start();
	}

	
//	public  void parseMapFileTest() {
//		String[] signal = null;
//		int gCounter = 0, counter = 0, notEvent = 0;
//		HashMap<String, Integer> hm = new HashMap<String, Integer>();
//		HashMap<String, Integer> wifiCounter = new HashMap<String, Integer>();
//		// open the file for reading
//		InputStream instream;
//		try {
//			instream = getAssets().open("signal.csv"); // openFileInput("signal2.csv");
//			// if file the available for reading
//			if (instream != null) {
//				// prepare the file for reading
//				InputStreamReader inputreader = new InputStreamReader(instream);
//				BufferedReader buffreader = new BufferedReader(inputreader);
//
//				String line;
//				MapPoint mp;
//
//				while ((line = buffreader.readLine()) != null) {
//
//					signal = line.split(",");
//
//					if (signal.length % 2 == 0) // if it is even number of elements
//					{
//						for (int i = 4; i < signal.length - 1; i += 2)
//						{
//							if (wifiCounter.containsKey(signal[i])) {
//								int temp = wifiCounter.get(signal[i]);
//								wifiCounter.put(signal[i], temp + 1);
//							} else {
//								wifiCounter.put(signal[i], 1);
//							}
//						}
//						/*
//						 * int xCor = Integer.parseInt(signal[1]); int yCor =
//						 * Integer.parseInt(signal[3]); int[] cors = {xCor,
//						 * yCor}; mp = new MapPoint( cors, hm); //create a map
//						 * point MapGenerator.getInstance().putPoint(mp);
//						 * for(int i = 0; i < signal.length; ++i) {
//						 * //System.out.println(signal[i]); }
//						 */
//					} else {
//						notEvent++;
//						System.out.println("Bad Line Number: " + counter);
//					}
//				}
//			}
//		} catch (FileNotFoundException e) {
//			e.printStackTrace();
//		} catch (IOException e) {
//			e.printStackTrace();
//		}
//
//		HashMap<String,Integer> newWifi = (HashMap<String, Integer>) wifiCounter.clone();
//		for (String s : wifiCounter.keySet()) {
//
//			if (wifiCounter.get(s) < wifiCounter.size()) {
//				newWifi.remove(s);
//			}
//		}
//		
//		
//		InputStream istream;
//		try {
//			istream = getAssets().open("signal.csv"); // openFileInput("signal2.csv");
//			// if file the available for reading
//			int counter2 = 0;
//			if (istream != null) {
//				// prepare the file for reading
//				InputStreamReader inputreader = new InputStreamReader(istream);
//				BufferedReader buffreader = new BufferedReader(inputreader);
//
//				String line;
//				MapPoint mp;
//
//				while ((line = buffreader.readLine()) != null) {
//					signal = line.split(",");
//					counter2++;
//					if (signal.length % 2 == 0) // if it is even number of elements
//					{
//						HashMap<String, Integer> wifiHm = new HashMap<String, Integer>();
//						for (int i = 4; i < signal.length - 1; i += 2) // point
//						{
//							if (newWifi.containsKey(signal[i])) {
//								wifiHm.put(signal[i], Integer.parseInt(signal[i+1]));
//							}
//						}
//						int xCor = Integer.parseInt(signal[1]); 
//						int yCor = Integer.parseInt(signal[3]); 
//						int[] cors = {xCor, yCor}; 
//						mp = new MapPoint(cors, wifiHm); //create a map
//						MapGenerator.getInstance().putPoint(mp);
//						//ArrayList<MapPoint> mps = MapGenerator.getInstance().getMap();
//						//System.out.println("msp is " + mps);
////						hm.clear();
////						ArrayList<MapPoint> mps = MapGenerator.getInstance().getMap();
////						System.out.println("msp is " + mps);
//						//System.out.println("msp wifi is " + mps.get(0).getWifiID());
//					}
//				}
//				
//				System.out.println("*********COUNT is *******" + counter2);
//				
//				ArrayList<MapPoint> mps = MapGenerator.getInstance().getMap();
//				
//				System.out.println("**********MAP size is ************************" + mps.size());
//				
//				for(MapPoint mpp: MapGenerator.getInstance().getMap())
//				{
//					System.out.println("Map point x: " + mpp.getCoordinates()[0] + " y: " + mpp.getCoordinates()[1]);
//
//				}
////				System.out.println("msp is " + mps);
//			}
//
//		} catch (FileNotFoundException e) {
//			e.printStackTrace();
//		} catch (IOException e) {
//			e.printStackTrace();
//		}
//	}

	private void autoDrive() {
		RoutePlanner.getInstance().search();
		String directive = RoutePlanner.getInstance().driveInstruction();
		System.out.println("DIR"+directive);
		String[] temp = directive.split(",");
		if (temp.length > 0) {
			for (int i = 0; i < temp.length; i++) {
				//btThread.bHandler.sendEmptyMessage(MSG_DELAY);
				if (temp[i].equals("forward")) {
					btThread.newDrive(1000);
					move(0, 1000);
				} else if (temp[i].equals("backward")) {
					btThread.newDrive(-1 * 1000);
					move(0, -1 * 1000);
					if (orientation ==90)
					{
						spinRight();
						btThread.newDrive(1000);
//						btThread.bHandler.sendEmptyMessage(MSG_DELAY);
						move(0, 1000);
					} else if (orientation ==270){
						spinLeft();
						btThread.newDrive(1000);
//						btThread.bHandler.sendEmptyMessage(MSG_DELAY);
						move(0, 1000);
					} else if (orientation == 180){
						btThread.newDrive(1000);
//						btThread.bHandler.sendEmptyMessage(MSG_DELAY);
						move(0, 1000);
					} else if(orientation == 0){
						
					}
				} else if (temp[i].equals("left")) {
					spinLeft();
//					btThread.bHandler.sendEmptyMessage(MSG_DELAY);
				} else if (temp[i].equals("right")) {
					spinRight();
//					btThread.bHandler.sendEmptyMessage(MSG_DELAY);
				}
			}
		} else {
			System.out.println("NO");
		}
	}
}
